/*******************************************************************************
* This file provides the functions to retrieve the following GPS information
* from the NMEA 0183 sentences:
* ~ Latitude and Longitude.
* ~ Altitude.
* ~ Speed over ground.
* ~ Course over ground.
* ~ HDOP.
* ~ Number of Satelites Fixed.
*
* This API is tested to be working with the LS20031 GPS module. For other
* GPS module, minor modification may be needed.
*
* Author: Kong Wai Weng
* Email:  waiweng83@gmail.com
*******************************************************************************/



#include "hal_gps.h"



/*******************************************************************************
* PRIVATE FUNCTION PROTOTYPES                                                  *
*******************************************************************************/

static void prv_vParseRmc(unsigned int uiReceivedData);
static void prv_vParseGga(unsigned int uiReceivedData);



/*******************************************************************************
* PRIVATE GLOBAL VARIABLES                                                     *
*******************************************************************************/

static volatile unsigned int uiNewDataAvailable = 0;
static volatile struct GPS_DATA xGpsData = {0};			// The complete GPS data.
static volatile struct GPS_DATA xGpsDataTemp = {0};		// Temporary buffer for calculation.

// Counters to indicate the current position in the NMEA 0183 sentences.
static volatile unsigned int uiCharacterCounter = 0;
static volatile unsigned int uiFieldCounter = 0;

// Indicate there is new data from GPS.
// This will be set when new data arrives,
// and will be cleared by the device watchdog.
static volatile unsigned int bGpsHeartbeat = 0;



/*******************************************************************************
* PUBLIC FUNCTION: xGetGpsData
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The GPS data.
*
* DESCRIPTIONS:
* Get the GPS data.
*
*******************************************************************************/
struct GPS_DATA xGetGpsData(void)
{
	struct GPS_DATA xBuffer;
	
	INTERRUPT_PROTECT(xBuffer = xGpsData);
	return xBuffer;
}	



/*******************************************************************************
* PUBLIC FUNCTION: vParseGpsData
*
* PARAMETERS:
* ~ uiReceivedData		- The current character received from the UART.
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Parse the GPS data to extract the relavant information.
*
*******************************************************************************/
void vParseGpsData(unsigned int uiReceivedData)
{	
	// Buffer for the NMEA header.
	static char szNmeaHeader[6] = {0};
	
	// Enum to represent the current sentence type.
	static enum {GPRMC, GPGGA, OTHERS} eSentenceType = OTHERS;
	
	// Flag to indicate the start and end of checksum calculation.
	static enum {START, END} eCalculateChecksum = END;
	
	// Buffer for calculated checksum.
	static unsigned int uiCalculatedChecksum = 0;
	
	// Buffer for received checksum.
	static unsigned int uiReceivedChecksum = 0;
	
	
	
	// Every new line of the NMEA 0183 sentences begins with a '$'.
	// If '$' is received, reset the GPS data buffers and counters.
	if (uiReceivedData == '$') {
		
		uiCharacterCounter = 0;
		uiFieldCounter = 0;
		
		// We also indicate the start of checksum calculation and reset the calculated checksum.
		eCalculateChecksum = START;
		uiCalculatedChecksum = 0;
	}
	
	// Every field in a sentence is separated by the ','.
	// If ',' is received, reset the character counter and increase the field counter.
	else if (uiReceivedData == ',') {
		uiCharacterCounter = 0;
		uiFieldCounter++;
	}
	
	// The last field is the checksum. It's separated by '*'.
	// If '*' is received, reset the character counter and increase the field counter.
	// We also set the flag to indicate the end of checksum calculation.
	else if (uiReceivedData == '*') {
		uiCharacterCounter = 0;
		uiFieldCounter++;
		
		eCalculateChecksum = END;
	}	
	
	// Every line of the NMEA 0183 sentences ends with "\r\n".
	// If '\n' is received and there is new data available,
	// verify the checksum and send the GPS data to the queue.
	else if ((uiReceivedData == '\n') && (uiNewDataAvailable == 1)) {
		
		// If the checksum match...
		if (uiCalculatedChecksum == uiReceivedChecksum) {
			// Copy the GPS data from the temporary buffer.
			if (eSentenceType == GPRMC) {
				
				INTERRUPT_PROTECT(	xGpsData.ucFixStatus = xGpsDataTemp.ucFixStatus;		\
									xGpsData.xDateTime = xGpsDataTemp.xDateTime;			\
									xGpsData.fLatitude = xGpsDataTemp.fLatitude;			\
									xGpsData.fLongitude = xGpsDataTemp.fLongitude;			\
									xGpsData.fGroundSpeed = xGpsDataTemp.fGroundSpeed;		\
									xGpsData.fGroundCourse = xGpsDataTemp.fGroundCourse;	\
								 );
				
				
				// Set the GPS heartbeat.
				// We set it here because the GPRMC data is the most important one.
				bGpsHeartbeat = 1;
			}	
			else if (eSentenceType == GPGGA) {
				INTERRUPT_PROTECT(	xGpsData.ucFixStatus = xGpsDataTemp.ucFixStatus;					\
									xGpsData.fAltitude = xGpsDataTemp.fAltitude;						\
									xGpsData.uiNumberOfSatelites = xGpsDataTemp.uiNumberOfSatelites;	\
									xGpsData.fHdop = xGpsDataTemp.fHdop;								\
								 );
			}	
		}	
    	
    	// Clear the new data available bit so that we only send once.
		uiNewDataAvailable = 0;
	}	
	
	// The rest of the characters are the data that we need to process.
	else {
		// Field 0 is the 5-character header of the NMEA 0183 sentences.
		// Store the string into the header buffer.
		if (uiFieldCounter == 0) {
			if ((uiCharacterCounter + 1) < sizeof(szNmeaHeader)) {
				szNmeaHeader[uiCharacterCounter] = uiReceivedData;
				szNmeaHeader[uiCharacterCounter + 1] = 0;
			}	
		    
		    // At the last character of this field, check the string to get the type of current sentence.
			if (uiCharacterCounter == (sizeof(szNmeaHeader) - 2)) {
				if (strcmp(szNmeaHeader, "GPRMC") == 0) {
					eSentenceType = GPRMC;
				}	
				else if (strcmp(szNmeaHeader, "GPGGA") == 0) {
					eSentenceType = GPGGA;
				}
				else {
					eSentenceType = OTHERS;
				}	
			} 
		}
		
		// Two characters after the '*' is received is the checksum.
		else if (eCalculateChecksum == END) {
			switch (uiCharacterCounter) {
				case 0:
					if ((uiReceivedData >= '0') && (uiReceivedData <= '9')) {
						uiReceivedChecksum = (uiReceivedData - 0x30) << 4;
					}	
					else if ((uiReceivedData >= 'A') && (uiReceivedData <= 'F')) {
						uiReceivedChecksum = (uiReceivedData - 0x37) << 4;
					}	
					break;
				
				case 1:
					if ((uiReceivedData >= '0') && (uiReceivedData <= '9')) {
						uiReceivedChecksum += (uiReceivedData - 0x30);
					}	
					else if ((uiReceivedData >= 'A') && (uiReceivedData <= 'F')) {
						uiReceivedChecksum += (uiReceivedData - 0x37);
					}
					break;
			}	
		}	
		
		// The rest of the data, we need to process them according to the header.
		else {
			// Parse the sentence.
			switch (eSentenceType) {
				case GPRMC:
					prv_vParseRmc(uiReceivedData);
					break;
				case GPGGA:
					prv_vParseGga(uiReceivedData);
					break;
				default:
					break;
			}	
		}	
	    
		
		
		// Increase the character counter.
		uiCharacterCounter++;
	}
	
	// Calculate the checksum if required.
	if ((eCalculateChecksum == START) && (uiReceivedData != '$')) {
		uiCalculatedChecksum ^= uiReceivedData;
	}	
	
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_vParseRmc
*
* PARAMETERS:
* ~ uiReceivedData		- The current character received from the UART.
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* In this function, the following information is extracted from the RMC sentence:
* ~ Time and Date.
* ~ GPS Fix Status.
* ~ Latitude and Longitude.
* ~ Speed over ground.
* ~ Course over ground.
*
*******************************************************************************/
static void prv_vParseRmc(unsigned int uiReceivedData)
{
	switch (uiFieldCounter) {
		
		// UTC Time (hhmmss.sss)
		case 1:
			switch (uiCharacterCounter) {
				// Hour.
				case 0:
					xGpsDataTemp.xDateTime.ucHour = (uiReceivedData - 0x30) * 10;
					break;
				case 1:
					xGpsDataTemp.xDateTime.ucHour += uiReceivedData - 0x30;
					break;
				
				// Minute.
				case 2:
					xGpsDataTemp.xDateTime.ucMinute = (uiReceivedData - 0x30) * 10;
					break;
				case 3:
					xGpsDataTemp.xDateTime.ucMinute += uiReceivedData - 0x30;
					break;
				
				// Second.
				case 4:
					xGpsDataTemp.xDateTime.ucSecond = (uiReceivedData - 0x30) * 10;
					break;
				case 5:
					xGpsDataTemp.xDateTime.ucSecond += uiReceivedData - 0x30;
					break;
				
				// Millisecond.
				case 7:
					xGpsDataTemp.xDateTime.uiMillisecond = (uiReceivedData - 0x30) * 100;
					break;
				case 8:
					xGpsDataTemp.xDateTime.uiMillisecond += (uiReceivedData - 0x30) * 10;
					break;
				case 9:
					xGpsDataTemp.xDateTime.uiMillisecond += uiReceivedData - 0x30;
					break;
			}	
			break;
			

			
		// GPS fix status.
		case 2:
			// Set the status of the GPS fix.
			xGpsDataTemp.ucFixStatus = (uiReceivedData == 'A')? 1 : 0;
			break;
		
		
		
		// Latitude (ddmm.mmmm).
		case 3:
			switch (uiCharacterCounter) {
				// Latitude degree.
				case 0:
					xGpsDataTemp.fLatitude = (float)(uiReceivedData - 0x30) * 10.0f;
					break;
				case 1:
					xGpsDataTemp.fLatitude += (float)(uiReceivedData - 0x30);
					break;
				
				// Latitude minute.
				case 2:
					xGpsDataTemp.fLatitude += (float)(uiReceivedData - 0x30) / 6.0f;
					break;
				case 3:
					xGpsDataTemp.fLatitude += (float)(uiReceivedData - 0x30) / 60.0f;
					break;
				
				// Latitude minute * 10^-4.
				case 5:
					xGpsDataTemp.fLatitude += (float)(uiReceivedData - 0x30) / 600.0f;
					break;
				case 6:
					xGpsDataTemp.fLatitude += (float)(uiReceivedData - 0x30) / 6000.0f;
					break;
				case 7:
					xGpsDataTemp.fLatitude += (float)(uiReceivedData - 0x30) / 60000.0f;
					break;
				case 8:
					xGpsDataTemp.fLatitude += (float)(uiReceivedData - 0x30) / 600000.0f;
					break;
			}	
			break;
		
		
		
		// Latitude sector (N / S).
		case 4:
			// We use negative value to represent South Hemisphere.
			if (uiReceivedData == 'S') {
				xGpsDataTemp.fLatitude = -xGpsDataTemp.fLatitude;
			}
			break;
		
		
		
		// Longitude (dddmm.mmmm).
		case 5:
			switch (uiCharacterCounter) {
				// Longitude degree.
				case 0:
					xGpsDataTemp.fLongitude = (float)(uiReceivedData - 0x30) * 100.0f;
					break;
				case 1:
					xGpsDataTemp.fLongitude += (float)(uiReceivedData - 0x30) * 10.0f;
					break;
				case 2:
					xGpsDataTemp.fLongitude += (float)(uiReceivedData - 0x30);
					break;
				
				// Longitude minute.
				case 3:
					xGpsDataTemp.fLongitude += (float)(uiReceivedData - 0x30) / 6.0f;
					break;
				case 4:
					xGpsDataTemp.fLongitude += (float)(uiReceivedData - 0x30) / 60.0f;
					break;
				
				// Longitude minute * 10^-4.
				case 6:
					xGpsDataTemp.fLongitude += (float)(uiReceivedData - 0x30) / 600.0f;
					break;
				case 7:
					xGpsDataTemp.fLongitude += (float)(uiReceivedData - 0x30) / 6000.0f;
					break;
				case 8:
					xGpsDataTemp.fLongitude += (float)(uiReceivedData - 0x30) / 60000.0f;
					break;
				case 9:
					xGpsDataTemp.fLongitude += (float)(uiReceivedData - 0x30) / 600000.0f;
					break;
			}	
			break;
		
		
		
		// Longitude sector (E / W).
		case 6:
			// We use negative value to represent West.
			if (uiReceivedData == 'W') {
				xGpsDataTemp.fLongitude = -xGpsDataTemp.fLongitude;
			}
			break;
		
		
		
		// Speed over ground.
		// The unit received here is in knots * 100.
		// We convert it to m/s when field 12 is received.
		case 7:
			switch (uiCharacterCounter) {
				case 0:
					xGpsDataTemp.fGroundSpeed = (float)(uiReceivedData - 0x30);
					break;
				default:
					if (uiReceivedData != '.') {
						xGpsDataTemp.fGroundSpeed *= 10.0f;
						xGpsDataTemp.fGroundSpeed += (float)(uiReceivedData - 0x30);
					}
					break;	
			}	
			break;
		
		
		
		// Course over ground.
		// The unit received here is in degree * 100.
		// We convert it to degree when field 12 is received.
		case 8:
			switch (uiCharacterCounter) {
				case 0:
					xGpsDataTemp.fGroundCourse = (float)(uiReceivedData - 0x30);
					break;
				default:
					if (uiReceivedData != '.') {
						xGpsDataTemp.fGroundCourse *= 10.0f;
						xGpsDataTemp.fGroundCourse += (float)(uiReceivedData - 0x30);
					}
					break;	
			}	
			break;
		
		
		
		// UTC Date (ddmmyy).
		case 9:
			switch (uiCharacterCounter) {
				// Date.
				case 0:
					xGpsDataTemp.xDateTime.ucDate = (uiReceivedData - 0x30) * 10;
					break;
				case 1:
					xGpsDataTemp.xDateTime.ucDate += uiReceivedData - 0x30;
					break;
				
				// Month.
				case 2:
					xGpsDataTemp.xDateTime.ucMonth = (uiReceivedData - 0x30) * 10;
					break;
				case 3:
					xGpsDataTemp.xDateTime.ucMonth += uiReceivedData - 0x30;
					break;
				
				// Year.
				case 4:
					xGpsDataTemp.xDateTime.ucYear = (uiReceivedData - 0x30) * 10;
					break;
				case 5:
					xGpsDataTemp.xDateTime.ucYear += uiReceivedData - 0x30;
					break;
			}	
			break;
		
		
		
		// We don't need the data here, but we process these data here:
		// ~ Ground Speed	- From knots * 100 to m/s.
		// ~ Ground Course	- From degree * 100 to degree.
		// ~ Change UTC into local time.
		// ~ Indicate new data is available.
		case 12:
			xGpsDataTemp.fGroundSpeed *= KNOTS_TO_MS * 0.01f;
			xGpsDataTemp.fGroundCourse *= 0.01f;
			
			// Change the UTC time into the local time by adding the time zone offset.
			// This calculation is only valid for positive time zone.
			// TODO: Must be valid for negative time zone as well.
			xGpsDataTemp.xDateTime.ucHour += TIME_ZONE;
			
			if (xGpsDataTemp.xDateTime.ucHour >= 24) {
				xGpsDataTemp.xDateTime.ucHour -= 24;
				xGpsDataTemp.xDateTime.ucDate++;
				
				switch (xGpsDataTemp.xDateTime.ucMonth) {
					case 1:
					case 3:
					case 5:
					case 7:
					case 8:
					case 10:
					case 12:
						if (xGpsDataTemp.xDateTime.ucDate > 31) {
							xGpsDataTemp.xDateTime.ucDate--;
							xGpsDataTemp.xDateTime.ucMonth++;
							
							if (xGpsDataTemp.xDateTime.ucMonth > 12) {
								xGpsDataTemp.xDateTime.ucMonth--;
								xGpsDataTemp.xDateTime.ucYear++;
							}	
						}
						break;	
					
					default:
						if (xGpsDataTemp.xDateTime.ucDate > 30) {
							xGpsDataTemp.xDateTime.ucDate--;
							xGpsDataTemp.xDateTime.ucMonth++;
						}
						break;
				}	
			}
			
			// Indicate new data is available.
			uiNewDataAvailable = 1;
			
			break;
		
	} // switch (uiFieldCounter)
}	



/*******************************************************************************
* PRIVATE FUNCTION: prv_vParseGga
*
* PARAMETERS:
* ~ uiReceivedData		- The current character received from the UART.
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* In this function, the following information is extracted from the GGA sentence:
* ~ Number of Satelites Fixed.
* ~ HDOP.
* ~ Altitude.
*
*******************************************************************************/
static void prv_vParseGga(unsigned int uiReceivedData)
{
	signed int siNegAlt = 0;
	
	switch (uiFieldCounter) {
		
		// GPS fix status.
		case 6:
			// Set the status of the GPS fix.
			xGpsDataTemp.ucFixStatus = (uiReceivedData == 0)? 0 : 1;
			break;
		
		
		
		// Number of satelites.
		case 7:
			switch (uiCharacterCounter) {
				case 0:
					xGpsDataTemp.uiNumberOfSatelites = uiReceivedData - 0x30;
					break;
				case 1:
					xGpsDataTemp.uiNumberOfSatelites *= 10;
					xGpsDataTemp.uiNumberOfSatelites += uiReceivedData - 0x30;
					break;
			}	
			break;
		
		
		
		// HDOP
		// The unit received here is in HDOP * 10.
		// We convert it to HDOP when field 10 is received.
		case 8:
			switch (uiCharacterCounter) {
				case 0:
					xGpsDataTemp.fHdop = (float)(uiReceivedData - 0x30);
					break;
				default:
					if (uiReceivedData != '.') {
						xGpsDataTemp.fHdop *= 10.0f;
						xGpsDataTemp.fHdop += (float)(uiReceivedData - 0x30);
					}
					break;	
			}	
			break;
		
		
		
		// Altitude
		// The unit received here is in m * 10.
		// We convert it to m when field 10 is received.
		case 9:
			switch (uiCharacterCounter) {
				case 0:
					if (uiReceivedData == '-') {
						// Indicate the altitude is negative.
						siNegAlt = 1;
						xGpsDataTemp.fAltitude = 0.0f;
					}	
					else {
						siNegAlt = 0;
						xGpsDataTemp.fAltitude = (float)(uiReceivedData - 0x30);
					}	
					break;
				default:
					if (uiReceivedData != '.') {
						xGpsDataTemp.fAltitude *= 10.0f;
						xGpsDataTemp.fAltitude += (float)(uiReceivedData - 0x30);
					}
					break;	
			}	
			break;
		
		
		
		// We don't need the data here, but we change the unit for these data here:
		// ~ Altitude	- From m * 10 to m. Also determine the signess here.
		// ~ HDOP		- From HDOP * 10 to HDOP.
		// We also indicate new data is available here.
		case 10:
			if (siNegAlt == 1) {
				xGpsDataTemp.fAltitude = -xGpsDataTemp.fAltitude;
			}
			xGpsDataTemp.fHdop *= 0.1f;
			xGpsDataTemp.fAltitude *= 0.1f;
			
			uiNewDataAvailable = 1;
			break;
		
	} // switch (uiFieldCounter)
}	



/*******************************************************************************
* TASK: taskGPSWatchdog
*
* DESCRIPTIONS:
* This task monitors the GPS heartbeat. If no GPS data is received
* after the timeout period, the error flag will be set.
*
*******************************************************************************/
portTASK_FUNCTION(taskGPSWatchdog, pvParameters)
{
	while (1) {
		// Reset the heartbeat.
		bGpsHeartbeat = 0;
		
		// Delay for 1 seconds.
		vTaskDelay(configTICK_RATE_HZ);
		
		// Check the heartbeat again.
		// Set the error flag if the heartbeat is not set.
		if (bGpsHeartbeat == 0) {
			xSystemError.bGpsError = 1;
			xGpsData.ucFixStatus = 0;
		}	
		else {
			xSystemError.bGpsError = 0;
		}
		
		
		
		// Record the stack high water mark.
		xStackWaterMark.uiGpsWatchdog = uxTaskGetStackHighWaterMark(NULL);
		
		// Set the error flag if the water mark is too low.
		if (xStackWaterMark.uiGpsWatchdog < RTOS_STACK_THRESHOLD) {
			xSystemError.bRtosStackError = 1;
		}
	}	
}	
